﻿//
// Created by qianchen on 2020/12/15.
//

#include "CascadeInfatryDetecter.h"

CascadeInfatryDetecter::CascadeInfatryDetecter(string & cascade_path):path(cascade_path)
{
    if(!Init())
    {
        cout << "[Cascade](!)CascadeInfatryDetecter failed to initialize!!\n";
    } else
        cout << "[Cascade]CascadeInfatryDetecter initialized\n";
}

CascadeInfatryDetecter::CascadeInfatryDetecter(string && cascade_path):path(cascade_path)
{
    if(!Init())
    {
        cout << "[Cascade](!)CascadeInfatryDetecter failed to initialize!!\n";
    }else
        cout << "[Cascade]CascadeInfatryDetecter initialized\n";
}

bool CascadeInfatryDetecter::Init()
{
    if( !infantry_cascade.load(path) )
    {
        cout << "[Cascade](!)Error loading infantry cascade!\n";
        return false;
    }
    else
        cout << "[Cascade]Loading infantry cascade!\n";
    return true;
}

bool CascadeInfatryDetecter::detect(Mat &frame, Rect prior_rec, bool vis)
{
    //判空
    if(frame.empty())
    {
        cout<<"[Cascade](!)Error loading this frame!\n";
        //return false;
    }
    if(prior_rec.x<0 || prior_rec.y<0 || prior_rec.x+prior_rec.width>frame.cols || prior_rec.y+prior_rec.height>frame.rows)
    {
        cout<<"[Cascade](!)Armor point missing!\n";
        //return false;
    }
    //灰度化：级联分类器针对灰度图
    Mat frame_gray;
    cvtColor( frame, frame_gray, COLOR_BGR2GRAY );
    //equalizeHist( frame_gray, frame_gray );//用于增强对比度，对于前景后景差别比较小的用处不大（反而容易都黑），如果场地是白色的可以考虑使用。
    int dx, dy;
    Mat ROI;
    getROIwithRect(frame_gray, prior_rec, ROI, dx, dy);
//    if (dx<0||dy<0)
//        return false;
//    if(ROI.empty())
//        return false;

    //调用级联分类器
    vector<int> rejectLevels;
    infantry_cascade.detectMultiScale( ROI, objects,rejectLevels,scores,1.05,3,0,Size(prior_rec.width*5,prior_rec.height*4.5),Size(prior_rec.width*7,prior_rec.height*6.5),true );
    //没找到之类的。。就判断失败
    if(objects.size()==0)
    {
        cout<<"[Cascade](!)Cannot find it (f**k!)\n";
        //return false;
    }



    for(auto &object : objects)
    {
        object.x += dx;
        object.y += dy;
        if(isInside(object,prior_rec))
        {
            if(object.height>theCorrectRect.height && object.width>theCorrectRect.height)
                theCorrectRect = object;
        }
        //cout<<object<<endl;
        //cout<<*scores.begin()<<endl;
    }

    if(vis)
        drawResult(frame, false);

    return true;

}

void CascadeInfatryDetecter::getROIwithRect(Mat frame, Rect prior_rec, Mat & ROI, int & dx, int & dy)
{
    //确定ROI左上角点
    int armorH = prior_rec.height;
    Point basePoint(prior_rec.x,prior_rec.y);
    Point lt=basePoint-Point(8*armorH,7*armorH);
    if(lt.x<0||lt.x>=frame.cols)
        lt.x = 0;
    if(lt.y<0||lt.x>=frame.cols)
        lt.y = 0;
    int widthROI = (lt.x+14*armorH<frame.cols)?14*armorH:frame.cols;
    int heightROI = (lt.y+14*armorH<frame.rows)?14*armorH:frame.rows;

    if (lt.x+widthROI > frame.cols)
        widthROI = frame.cols - lt.x;
    if (lt.y+heightROI> frame.rows)
        heightROI = frame.rows - lt.y;

    //得到ROI
    Rect recROI(lt,Size(widthROI,heightROI));
    if (widthROI==0||heightROI==0)
    {
        ROI = frame;
        lt.x = 0;
        lt.y = 0;
    }
    else
    {
        ROI = frame(recROI);
    }
    dx = lt.x;
    dy = lt.y;
    cout << "dx:: "<<dx << "   " << "dy :: " << dy << endl;
    imshow("sasas",ROI);

}
